#include <iostream>
#include <thread>

#include <opencv2/opencv.hpp>

extern "C" {
#include "apriltag.h"
#include "apriltag_pose.h"
#include "tag36h11.h"
#include "tag25h9.h"
#include "tag16h5.h"
#include "tagCircle21h7.h"
#include "tagCircle49h12.h"
#include "tagCustom48h12.h"
#include "tagStandard41h12.h"
#include "tagStandard52h13.h"
#include "common/getopt.h"
}

using namespace std;
using namespace cv;
int main(int argc, char **argv) {
    
    string imgPath  = "/home/john/LJJ_SRC/OpenCV/opencv_test/Apritag_test/2.png";

    apriltag_detection_info_t info;
    // info.det = det;
    info.tagsize = 0.144;
    info.fx = 385.613;
    info.fy = 385.613;
    info.cx = 321.183;
    info.cy = 240.455;


    Mat img, gray;
    img = imread(imgPath);
    cvtColor(img, gray, COLOR_BGR2GRAY);

    image_u8_t img_header = { .width = gray.cols,
        .height = gray.rows,
        .stride = gray.cols,
        .buf = gray.data
    };

    apriltag_family_t *tf = tag36h11_create();
    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    
    zarray_t *detections = apriltag_detector_detect(td, &img_header);
    cout << "detections  " << zarray_size(detections) <<endl;

    
    for (int i = 0; i < zarray_size(detections); i++) {

        apriltag_detection_t *det;
        zarray_get(detections, i, &det);

        // Draw detection outlines
        // p[4][2]：第一维元素左下角开始逆时针0123，第二维元素为像素坐标
        // c[2]：中心点的像素坐标
        line(img, Point(det->p[0][0], det->p[0][1]),
                    Point(det->p[1][0], det->p[1][1]),
                    Scalar(255, 0, 255), 2);
        line(img, Point(det->p[0][0], det->p[0][1]),
                    Point(det->p[3][0], det->p[3][1]),
                    Scalar(255, 0, 255), 2);
        line(img, Point(det->p[1][0], det->p[1][1]),
                    Point(det->p[2][0], det->p[2][1]),
                    Scalar(255, 0, 255), 2);
        line(img, Point(det->p[2][0], det->p[2][1]),
                    Point(det->p[3][0], det->p[3][1]),
                    Scalar(255, 0, 255), 2);

        arrowedLine(img, Point(det->c[0], det->c[1]),
                    Point((det->p[2][0] + det->p[1][0])/2, (det->p[2][1] + det->p[1][1])/2),
                    Scalar(0, 0, 255), 2);
        arrowedLine(img, Point(det->c[0], det->c[1]),
                    Point((det->p[0][0] + det->p[1][0])/2, (det->p[0][1] + det->p[1][1])/2),
                    Scalar(0, 255, 0), 2);

        stringstream ss;
        ss << det->id;
        String text = ss.str();
        int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;
        double fontscale = 1.0;
        int baseline;
        Size textsize = getTextSize(text, fontface, fontscale, 2,
                                        &baseline);
        putText(img, text, Point(det->c[0]-textsize.width/2,
                                    det->c[1]+textsize.height/2),
                fontface, fontscale, Scalar(255, 0, 255), 2);

        // Get pose
        info.det = det;
        apriltag_pose_t pose;
        double err = estimate_tag_pose(&info, &pose);
        cout << "\r\n----------- " << "ID: " << det->id << " -----------" << endl;
        /*  
        坐标系的原点位于摄像机中心, z轴从摄像机中心指向摄像机镜头外, x轴向右，y轴向下。
        标记的坐标系以标记的中心为中心，x轴向右，y轴向下，z轴进入标记。
        求得的是相机坐标系到标记中心坐标系的位姿变换。
        */
        cout << pose.R->data[0] << "  " << pose.R->data[1] << " " << pose.R->data[2]  << " " << pose.t->data[0] << endl;
        cout << pose.R->data[3] << "  " << pose.R->data[4] << " " << pose.R->data[5]  << " " << pose.t->data[1] << endl;
        cout << pose.R->data[6] << "  " << pose.R->data[7] << " " << pose.R->data[8]  << " " << pose.t->data[2] << endl;


        // cout << pose.t->data[0] << "  " << pose.t->data[1] << " " << pose.t->data[2] << endl;
        // cout << pose.R->nrows << " " << pose.R->ncols << endl;  //输出R，3*3
    }


    imshow("Tag Detections", img);
    waitKey(0);
    // Cleanup.
    tag36h11_destroy(tf);
    apriltag_detector_destroy(td);

    // while(1){
        
    // }


    return 0;
}

